#include <iostream>
#include <libplayerc++/playerc++.h>

#define PAREDE_A_ESQUERDA 1
#define PAREDE_A_DIREITA 2

int main(int argc, char *argv[])
{
  using namespace PlayerCc;

  PlayerClient robot("localhost");
  Position2dProxy pp(&robot,0);
  LaserProxy lp(&robot,0);
  pp.SetMotorEnable(1);

  double v_x, v_y, w;

  //Calibra os valores das coordenadas do Robo
  double x = 0, target_x = 6.0, error_x = 0;
  double y = 0, target_y = -3.0, error_y = 0;
  double theta = 0, target_theta = 3.14, error_theta = 0;

  // usado para definir um número máximo de iterações
  int iter = 0;
 
  //Lê a localização do Robo
  pp.SetOdometry(x, y, theta); 
  
  //Atualiza os valores
  do {
	  //Lê todos os parametros do robo
	  robot.Read();

  } while((pp.GetXPos() != x) || (pp.GetYPos() != y) || (pp.GetYaw() != theta));
 
  do {
  
	error_x = target_x - x;
  	error_y = target_y - y;
  	error_theta = target_theta - theta;
 
  	v_x = error_x * cos(theta) + error_y * sin(theta);
  	v_y = - error_x * sin(theta) + error_y * sin(theta);
  	w = error_theta;
 
  	pp.SetSpeed(v_x, v_y, w);
 
  	robot.Read();
 
  	x = pp.GetXPos();
  	y = pp.GetYPos();
  	theta = pp.GetYaw();

  } while((iter++ < 100) && !((fabs(error_x) == 0.0) && (fabs(error_y) == 0.0) && (fabs(error_theta) == 0.0)));


  float velocidade, ratacao, aX, aY, aYaw, aGx, aGy, aGa;
  
  velocidade = 1; 
  ratacao = 0;
   
  pp.SetSpeed(velocidade, ratacao);

  while(1){
  	robot.Read();
        printf("Velocidade : %f \n", velocidade);
        void 	SetGoalPose (double aGx, double aGy, double aGa);
	printf("Coordenadas : %f, %f, %f \n",aGx, aGy, aGa);
       // printf("Posição : %i \n", pp);

	if((lp[210]<1)&&(velocidade!=0)) //Ve se o lado que o lase esta mais para direita
	{
		velocidade=0.1; //Velocidade quando encontra um obstáculo
		ratacao=-1; //Velocidade de virar quando encontra um obstáculo
		printf("else 1 \n");
	}
	else if((lp[180]<1))
	{
		velocidade = -0.1;
	}
	else if((lp[160]<1)&&(velocidade!=0)) //Ve se o lado que o lase esta mais para esquerda 
	{	
		velocidade=0.1; //Velocidade quando encontra um obstáculo
		ratacao=1; //Velocidade virar quando encontra um obstáculo
		printf("else 2 \n");
	}
	else if((velocidade=0)&&(lp[210]<1))
	{
		velocidade=-0.1; //Velocidade quando encontra um obstáculo
		ratacao=2; //Velocidade de virar quando encontra um obstáculo
	}
	else if((velocidade=0)&&(lp[160]<1)) //Ve se o lado que o lase esta mais para esquerda 
	{	
		velocidade=-0.1; //Velocidade quando encontra um obstáculo
		ratacao=-2; //Velocidade virar quando encontra um obstáculo
	}
	else               // Senão anda normal
	{
		velocidade=2; //Velocidade quando não tem obstaculo
		ratacao=0; //Velocidade virar quando não tem obstaculo
	}
	pp.SetSpeed(velocidade,ratacao);
  }
  	
}

